//
// Created by fanghr on 18-3-28.
//

#include "../include/CameraCalibrator.h"

//打开棋盘图并识别交点
int CameraCalibrator::addChessboardPoints(
        const std::vector<std::string> &filelist, //棋盘图文件列表
        cv::Size &boardSize,                      //棋盘尺寸
        std::string windowName) {                                            //显示结果的图像窗口名字
    //如果为空则不显示
    //棋盘上的点
    std::vector<cv::Point2f> imageCorners;  //二维
    std::vector<cv::Point3f> objectCorners; //三维

    //三维点初始化
    // (X,Y,Z)= (i,j,0)
    for (int i = 0; i < boardSize.height; i++) {
        for (int j = 0; j < boardSize.width; j++) {

            objectCorners.push_back(cv::Point3f(i, j, 0.0f));
        }
    }

    //获取二维点
    cv::Mat image;
    int successes = 0;
    for (int i = 0; i < filelist.size(); i++) {

        image = cv::imread(filelist[i], 0);

        bool found = cv::findChessboardCorners(
                image, boardSize, imageCorners);

        //亚像素级精度
        cv::cornerSubPix(image, imageCorners,
                         cv::Size(5, 5),
                         cv::Size(-1, -1),
                         cv::TermCriteria(cv::TermCriteria::MAX_ITER +
                                          cv::TermCriteria::EPS,
                                          40,
                                          0.001));

        //完整棋盘则加入向量
        if (imageCorners.size() == boardSize.area()) {
            addPoints(imageCorners, objectCorners);
            successes++;
        }

        if (windowName.length() > 0) {
            //显示
            cv::drawChessboardCorners(image, boardSize, imageCorners, found);
            cv::imshow(windowName, image);
            cv::waitKey(100);
        }
    }

    return successes;
}

void CameraCalibrator::addPoints(const std::vector<cv::Point2f> &imageCorners,
                                 const std::vector<cv::Point3f> &objectCorners) {
    imagePoints.push_back(imageCorners);
    objectPoints.push_back(objectCorners);
}

//校准
double CameraCalibrator::calibrate(const cv::Size imageSize) {
    mustInitUndistort = true;
    std::vector<cv::Mat> rvecs, tvecs;
    return cv::calibrateCamera(objectPoints, //三维点
                               imagePoints,  // 二维点
                               imageSize,    // 尺寸
                               cameraMatrix, // 相机矩阵
                               distCoeffs,   // 畸变矩阵
                               rvecs, tvecs, // Rs, Ts
                               flag);         // 选项
}

//重映射图像
cv::Mat CameraCalibrator::remap(const cv::Mat &image) {

    cv::Mat undistorted;

    if (mustInitUndistort) {

        cv::initUndistortRectifyMap(
                cameraMatrix,
                distCoeffs,
                cv::Mat(),
                cv::Mat(),
                image.size(),
                CV_32FC1,
                map1, map2);
        //获得xy的映射矩阵
        mustInitUndistort = false;
    }

    //映射
    cv::remap(image, undistorted, map1, map2,
              cv::INTER_LINEAR);

    return undistorted;
}

void CameraCalibrator::setCalibrationFlag(bool radial8CoeffEnabled, bool tangentialParamEnabled) {
    flag = 0;
    if (!tangentialParamEnabled)
        flag += CV_CALIB_ZERO_TANGENT_DIST;
    if (radial8CoeffEnabled)
        flag += CV_CALIB_RATIONAL_MODEL;
}
